/**
 *
 */

import lejos.navigation.TachoNavigator;
import lejos.nxt.LightSensor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.comm.RConsole;
import lejos.subsumption.Behavior;

/**
 * @author Mathias
 *
 */
public class Drive implements Behavior {

	private TachoNavigator tn;
	private State state;

	public Drive(TachoNavigator tn, State state)
	{
		this.tn = tn;
		this.state = state;
	}

	public boolean takeControl() {
		int avgdist = 0;

		try {
			// get last 8 distance measurements
			UltrasonicSensor sonar = new UltrasonicSensor(SensorPort.PORTS[3]);
			int[] distances = new int[8];
			sonar.continuous();
			try {
			Thread.sleep(40);
			} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			}
			sonar.getDistances(distances);

			// compute average distance

			for (int i = 0; i < distances.length; i++) {
				RConsole.println("distances[" + i + "]" + new Integer(distances[i]).toString());
				avgdist += distances[i];
			}
			avgdist = avgdist / 8;
			RConsole.println(new Integer(avgdist).toString());
			RConsole.println("=============================");

			// check condition

		} catch (Exception e) {
			System.out.println(e.getMessage());
		}

		return avgdist >= 20;
		//return true;
	}

	public void suppress() {
		tn.stop();
	}

	public void action() {
		tn.goTo(100, 0);
		tn.goTo(100, 100);
		tn.goTo(0, 100);
		tn.goTo(0, 0);
	}

}
